///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef __BUOLA_CV_COCTREE_H__
#define __BUOLA_CV_COCTREE_H__

#include <buola/mat.h>
#include <buola/cv/coctreedefaultkey.h>
#include <buola/cv/coctreedefaultnode.h>
#include <buola/iterator/facade.h>

namespace buola { namespace cvi {
    
template<typename tData,typename tNode=COcTreeDefaultNode<tData>,typename tKey=COcTreeDefaultKey>
class COcTree
{
public:
    static const size_t cMaxDepth=tKey::cMaxDepth;

    typedef tKey TKey;
    typedef tNode TNode;

private:
    struct STreeIteratorChecker
    {
        template<typename tIt>
        bool Check(const tIt&)
        {
            return true;
        }
    };
    
    struct SLeafIteratorChecker
    {
        template<typename tIt>
        bool Check(const tIt &pIt)
        {
            return pIt->HasData();
        }
    };

    struct SVoxelIteratorChecker
    {
        template<typename tIt>
        bool Check(const tIt &pIt)
        {
            return pIt.Depth()==cMaxDepth;
        }
    };

    template<typename tChecker,bool vConst>
    class SIterator : public IFacade<SIterator<tChecker,vConst>,std::forward_iterator_tag,
                                     typename std::conditional<vConst,const tNode,tNode>::type>,public tChecker
    {
        friend class IFacade<SIterator<tChecker,vConst>,std::forward_iterator_tag,
                             typename std::conditional<vConst,const tNode,tNode>::type>;

        typedef typename std::conditional<vConst,const tNode*,tNode*>::type TPointer;
        typedef typename std::conditional<vConst,const tNode&,tNode&>::type TReference;
                             
        struct SElement
        {
            TPointer mNode;
            tKey mKey;
            int mChild;
        };

    public:
        ///default constructor

        ///Leaves the iterator in an invalid state
        SIterator()
            :   mTree(nullptr)
            ,   mNode(nullptr)
        {
        }

        SIterator(const SIterator&)=default;

        mat::CVec3f Coords() const  {   return mKey.Coords(mTree->Resolution(),Depth());    }
        const tKey &Key() const     {   return mKey;                                        }
        int Depth() const           {   return mDepth;                                      }

    private:
        /// initializes the iterator from a \c tValue
        SIterator(const COcTree *pTree,TPointer pNode,const tKey &pKey)
            :   mTree(pTree)
            ,   mDepth(0)
            ,   mNode(pNode)
            ,   mKey(pKey)
        {
            if(mNode&&!this->Check(*this))
                Increment();
        }

        /// returns the current value of the counter.
        TReference &Dereference() const
        {
            return *mNode;
        }

        /// increments the value of the iterator
        void Increment()
        {
            do
            {
                if(mNode->HasChildren())
                {
                    mStack[mDepth].mNode=mNode;
                    mStack[mDepth].mKey=mKey;

                    int i=0;
                    while(!mNode->HasChild(i)) i++;

                    mStack[mDepth].mChild=i;
                    mNode=mNode->Child(i);
                    mKey=mKey.Child(i,mDepth);
                    mDepth++;
                }
                else
                {
                    int i;
                    for(i=mDepth-1;i>=0;i--)
                    {
                        //find next existing child
                        int j=mStack[i].mChild+1;
                        while(j<8&&!mStack[i].mNode->HasChild(j)) j++;

                        if(j<8)
                        {
                            mStack[i].mChild=j;
                            mDepth=i+1;
                            mNode=mStack[i].mNode->Child(j);
                            mKey=mStack[i].mKey.Child(j,i);
                            break;
                        }
                    }
                    
                    if(i<0)
                    {
                        mNode=nullptr;
                        return;
                    }
                }
            } while(!this->Check(*this));
        }

        ///returns true if the two iterators contain the same value

        ///All end iterators compare equal
        bool Equal(const SIterator &pO) const
        {
            return (mNode==pO.mNode);
        }

        const COcTree *mTree;
        SElement mStack[tKey::cMaxDepth];
        int mDepth;
        TPointer mNode;
        tKey mKey;

        friend class COcTree;
    };

public:
    typedef SIterator<SLeafIteratorChecker,false> iterator;
    typedef SIterator<SLeafIteratorChecker,true> const_iterator;
    typedef SIterator<STreeIteratorChecker,false> tTreeIterator;
    typedef SIterator<SVoxelIteratorChecker,false> tVoxelIterator;
    
    explicit COcTree(float pResolution=1.0)
        :   mResolution(pResolution)
        ,   mSize(1)
    {}

    COcTree(COcTree &&pO)
        :   mRoot(std::move(pO.mRoot))
        ,   mResolution(pO.mResolution)
        ,   mSize(pO.mSize)
    {
        pO.mSize=1;
    }
    
    COcTree(const COcTree&)=delete;

    COcTree(NClone,const COcTree &pO)
        :   mRoot(pO.mRoot)
        ,   mResolution(pO.mResolution)
        ,   mSize(pO.mSize)
    {
        msg_info() << "cloning octree\n";
    }
    
public:    
    ~COcTree()
    {}

    size_t MaxDepth() const     {   return cMaxDepth;       }
    float Resolution() const   {   return mResolution;     }
    const tNode &Root() const   {   return mRoot;           }
    tNode &Root()               {   return mRoot;           }
    size_t size() const         {   return mSize;           }
    bool empty() const          {   return !mSize;          }
    
    const tNode *Search(const mat::CVec3f &pValue) const;

    tNode &GetNode(const tKey &pKey,int pDepth=cMaxDepth);

    tNode &GetNode(const mat::CVec3f &pPoint,int pDepth=cMaxDepth)
    {
        return GetNode(tKey(pPoint,mResolution),pDepth);
    }
    
    void Expand()
    {
        ExpandRecurs(&mRoot,0,cMaxDepth);
    }
    
    void Prune()
    {
    }

    void clear()
    {
        mRoot=tNode();
        mSize=1;
    }

    iterator begin()
    {
        return iterator(this,&mRoot,tKey::Root());
    }

    iterator end()
    {
        return iterator(this,nullptr,tKey());
    }
    
    const_iterator begin() const
    {
        return const_iterator(this,&mRoot,tKey::Root());
    }

    const_iterator end() const
    {
        return const_iterator(this,nullptr,tKey());
    }

    tTreeIterator TreeBegin()
    {
        return tTreeIterator(this,&mRoot,tKey::Root());
    }
    
    tTreeIterator TreeEnd()
    {
        return tTreeIterator(this,nullptr,tKey());
    }

    tTreeIterator VoxelBegin()
    {
        return tTreeIterator(this,&mRoot,tKey::Root());
    }
    
    tTreeIterator VoxelEnd()
    {
        return tTreeIterator(this,nullptr,tKey());
    }

    std::vector<tKey> ComputeRayKeys(const mat::CVec3f &pFrom,const mat::CVec3f &pTo) const;
    tKey ComputeKey(const mat::CVec3f &pPoint) const
    {
        return tKey(pPoint,mResolution);
    }

    COcTree &operator=(COcTree &&pRH)
    {
        mRoot=std::move(pRH.mRoot);
        mResolution=pRH.mResolution;
        mSize=pRH.mSize;
        pRH.mSize=1;

        return *this;
    }
    
private:
    void ExpandRecurs(tNode *pNode,int pDepth,int pMaxDepth);

    friend COcTree clone(const COcTree &pO)
    {
        return pO;
    }

private:    
    tNode mRoot;

    float mResolution;
    size_t mSize; ///< number of nodes in tree
};

template<typename tData,typename tNode,typename tKey>
const tNode *COcTree<tData,tNode,tKey>::Search(const mat::CVec3f &pValue) const
{
    tKey lKey(pValue,mResolution);
    if(!lKey.Valid())
        throw XRange("argument to COcTree::Search is out of bounds");

    const tNode *lNode=&mRoot;
//        size_t lPos=0;

    for(int d=0;d<cMaxDepth;d++)
    {
        uint64_t lPos=lKey.ChildPos(d);

        if(lNode->HasChild(lPos))
            lNode=lNode->Child(lPos);
        else if(!lNode->HasChildren()) //it is a leaf
            return lNode;
        else //search failed
            return nullptr;
    }
    return lNode;
}

template<typename tData,typename tNode,typename tKey>
tNode &COcTree<tData,tNode,tKey>::GetNode(const tKey &pKey,int pDepth)
{
    tNode *lNode=&mRoot;
    int lDepth=0;

    while(lDepth<pDepth)
    {
        if(lNode->HasData())
        {
            lNode->Expand();
            mSize+=8;
            lNode=lNode->Child(pKey.ChildPos(lDepth));
        }
        else if(lNode->HasChildren())
        {
            tNode *lChild=lNode->Child(pKey.ChildPos(lDepth));
            if(lChild)
                lNode=lChild;
            else
            {
                lNode=lNode->CreateChild(pKey.ChildPos(lDepth));
                mSize++;
            }
        }
        else
        {
            lNode=lNode->CreateChild(pKey.ChildPos(lDepth));
            mSize++;
        }
        lDepth++;
    }

    return *lNode;
}

template<typename tData,typename tNode,typename tKey>
void COcTree<tData,tNode,tKey>::ExpandRecurs(tNode *pNode,int pDepth,int pMaxDepth)
{
    if(!pNode->HasChildren())
    {
        pNode->Expand();
        mSize+=8;
    }

    if(pDepth+1<pMaxDepth)
    {
        for(int i=0;i<8;i++)
        {
            ExpandRecurs(pNode->Child(i),pDepth+1,pMaxDepth);
        }
    }
}

template <typename tData,typename tNode,typename tKey>
std::vector<tKey> COcTree<tData,tNode,tKey>::ComputeRayKeys(const mat::CVec3f &pFrom,const mat::CVec3f &pTo) const
{
    std::vector<tKey> lRay;

    tKey lFromKey(pFrom,mResolution);
    tKey lToKey(pTo,mResolution);

    if(!lFromKey.Valid()||!lToKey.Valid())
        return lRay;

    if(lFromKey==lToKey) return lRay;

    mat::CVec3f lDirection=pTo-pFrom;
    float lLength=norm(lDirection);
    lDirection/=lLength;
    
    int    lStep[3];
    float lTMax[3];
    float lTDelta[3];

    tKey lKey=lFromKey;

    mat::CVec3f lFromCenter=lKey.Coords(mResolution);
    
    for(int i=0;i<3;i++)
    {
        lStep[i]=get_sign<int>(lDirection[i]);
        if(lStep[i]!=0)
        {
            float lBorder=lFromCenter[i]+lStep[i]*mResolution*0.5f;
            lTMax[i]=(lBorder-pFrom[i])/lDirection[i];
            lTDelta[i]=mResolution/abs(lDirection[i]);
        }
        else
        {
            lTMax[i]=std::numeric_limits<float>::max();
        }
    }

    while(true)
    {
        ///reconsider if we should take equality into account
        if(lTMax[0]<lTMax[1])
        {
            if(lTMax[0]<lTMax[2]) //lTMax[0] is min
            {
                if(lTMax[0]>lLength) break;
                lRay.push_back(lKey);
                lKey.IncX(lStep[0]);
                lTMax[0]+=lTDelta[0];
            }
            else
            {
                if(lTMax[2]>lLength) break;
                lRay.push_back(lKey);
                lKey.IncZ(lStep[2]);
                lTMax[2]+=lTDelta[2];
            }
        }
        else
        {
            if(lTMax[1]<lTMax[2])
            {
                if(lTMax[1]>lLength) break;
                lRay.push_back(lKey);
                lKey.IncY(lStep[1]);
                lTMax[1]+=lTDelta[1];
            }
            else
            {
                if(lTMax[2]>lLength) break;
                lRay.push_back(lKey);
                lKey.IncZ(lStep[2]);
                lTMax[2]+=lTDelta[2];
            }
        }

        if(lKey==lToKey)
            break;
            
    }

    return lRay;
}

template<typename tData,typename tNode,typename tKey>
std::pair<mat::CVec3f,mat::CVec3f> get_minmax(const COcTree<tData,tNode,tKey> &pTree)
{
    mat::CVec3f lMin{std::numeric_limits<float>::max(),std::numeric_limits<float>::max(),
                   std::numeric_limits<float>::max()};
    mat::CVec3f lMax{std::numeric_limits<float>::min(),std::numeric_limits<float>::min(),
                   std::numeric_limits<float>::min()};

    const std::size_t lMaxDepth=COcTree<tData,tNode,tKey>::cMaxDepth;
    
    float lHalfSizes[lMaxDepth+1];
    lHalfSizes[lMaxDepth]=pTree.Resolution()/2.0f;
    float lHalfSize=pTree.Resolution();
    for(int i=lMaxDepth-1;i>=0;i--)
    {
        lHalfSizes[i]=lHalfSize;
        lHalfSize*=2.0f;
    }
        
    
    for(auto i=pTree.begin();i!=pTree.end();++i)
    {
        mat::CVec3f lReal=i.Coords();
        float lInc=lHalfSizes[i.Depth()];

        mat::CVec3f lPoint({lReal.x()-lInc,lReal.y()-lInc,lReal.z()-lInc});
        lMin.x()=std::min(lMin.x(),lPoint.x());
        lMin.y()=std::min(lMin.y(),lPoint.y());
        lMin.z()=std::min(lMin.z(),lPoint.z());
        lMax.x()=std::max(lMax.x(),lPoint.x());
        lMax.y()=std::max(lMax.y(),lPoint.y());
        lMax.z()=std::max(lMax.z(),lPoint.z());
    }

    return {lMin,lMax};
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif
